{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "intro_md"
      },
      "source": [
        "# PoseRotationPrior"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "desc_md"
      },
      "source": [
        "`PoseRotationPrior<POSE>` is a unary factor that applies a prior constraint only to the **rotation** component of a `POSE` variable (e.g., `Pose2` or `Pose3`).\n",
        "It ignores the translation component of the pose variable during error calculation.\n",
        "The error is calculated as the difference between the rotation component of the pose variable and the measured prior rotation, expressed in the tangent space of the rotation group.\n",
        "\n",
        "Error: $ \\text{Log}(\\text{measured}^{-1} \\cdot \\text{pose.rotation}()) $\n",
        "\n",
        "This is useful when you have information about the absolute orientation of a pose but little or no information about its translation."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "colab_badge_md"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/PoseRotationPrior.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "pip_code",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "%pip install --quiet gtsam-develop"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "id": "imports_code"
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "from gtsam import Pose3, Rot3, Point3, Values, PoseRotationPrior3D\n",
        "from gtsam import symbol_shorthand\n",
        "\n",
        "X = symbol_shorthand.X"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "create_header_md"
      },
      "source": [
        "## Creating a PoseRotationPrior"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "create_desc_md"
      },
      "source": [
        "Provide the key of the pose variable, the measured prior rotation (`Rot3` for `Pose3`, `Rot2` for `Pose2`), and a noise model defined on the rotation manifold's dimension (e.g., 3 for `Rot3`)."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "id": "create_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "PoseRotationPrior: PoseRotationPrior  keys = { x0 }\n",
            "isotropic dim=3 sigma=0.05\n",
            "Measured Rotation [\n",
            "\t0.707107, -0.707107, 0;\n",
            "\t0.707107, 0.707107, 0;\n",
            "\t0, 0, 1\n",
            "]\n"
          ]
        }
      ],
      "source": [
        "pose_key = X(0)\n",
        "measured_rotation = Rot3.Yaw(np.pi / 4) # Prior belief about orientation\n",
        "\n",
        "# Noise model on rotation (3 dimensions for Rot3)\n",
        "rotation_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.05) # 0.05 radians std dev\n",
        "\n",
        "# Factor type includes the Pose type, e.g. PoseRotationPrior3D\n",
        "factor = PoseRotationPrior3D(pose_key, measured_rotation, rotation_noise)\n",
        "factor.print(\"PoseRotationPrior: \")\n",
        "\n",
        "# Alternative constructor: extract rotation from a full Pose3 prior\n",
        "full_pose_prior = Pose3(measured_rotation, Point3(10, 20, 30)) # Translation is ignored\n",
        "factor_from_pose = PoseRotationPrior3D(pose_key, full_pose_prior, rotation_noise)\n",
        "# factor_from_pose.print(\"\\nFrom Pose Prior: \") # Should be identical"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "eval_header_md"
      },
      "source": [
        "## Evaluating the Error"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "eval_desc_md"
      },
      "source": [
        "The error depends only on the rotation part of the `Pose3` value in the `Values` object."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {
        "id": "eval_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Error with correct rotation: 0.0 (Should be near zero)\n",
            "Error with incorrect rotation: 1.9999999999999951 (Should be non-zero)\n",
            "Error with different translation: 1.9999999999999951 (Should be same as error2)\n"
          ]
        }
      ],
      "source": [
        "values = Values()\n",
        "\n",
        "# Pose with correct rotation but different translation\n",
        "pose_val1 = Pose3(measured_rotation, Point3(1, 2, 3))\n",
        "values.insert(pose_key, pose_val1)\n",
        "error1 = factor.error(values)\n",
        "print(f\"Error with correct rotation: {error1} (Should be near zero)\")\n",
        "\n",
        "# Pose with incorrect rotation\n",
        "pose_val2 = Pose3(Rot3.Yaw(np.pi / 4 + 0.1), Point3(1, 2, 3))\n",
        "values.update(pose_key, pose_val2)\n",
        "error2 = factor.error(values)\n",
        "print(f\"Error with incorrect rotation: {error2} (Should be non-zero)\")\n",
        "\n",
        "# Check that translation change doesn't affect error\n",
        "pose_val3 = Pose3(Rot3.Yaw(np.pi / 4 + 0.1), Point3(100, 200, 300))\n",
        "values.update(pose_key, pose_val3)\n",
        "error3 = factor.error(values)\n",
        "print(f\"Error with different translation: {error3} (Should be same as error2)\")\n",
        "assert np.allclose(error2, error3)"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "gtsam",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.13.1"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}